#include "WheeledShifter.h"
#include "util/Util.h"
WheeledShifter::WheeledShifter(Assembler* pAssm):
Shifter(pAssm),
m_serial(this)
{

}

WheeledShifter::~WheeledShifter()
{

}


ErrorCode WheeledShifter::Init()
{
	bool bOk = m_config.SetSource("./config/wheeledshifter.cfg");
	if( !bOk )
	{
		_LOG("Can't Open wheeledshifter.cfg");
		return eFailed;
	}
	_LOG("Init Ser OK!");
	std::string strAdd = m_config.GetStringDefault("SERIAL","PORT","");
	bool bIsOk = m_serial.InitSession(strAdd.c_str(),"","");
	if( !bIsOk )
	{
		_LOG("connect [%s] error!",strAdd.c_str() );
		return eFailed;
	}
	_LOG("WheeledShifter Init Success...");
	this->OnReady(0);
	return eOK;
}


ErrorCode WheeledShifter::PushData(char* pData, uint32 nLen)
{
	m_serial.OutPacket(0,nLen,pData);
	return eOK;
}
ErrorCode WheeledShifter::OnReceive(const char* pData,uint32 nLen) 
{
	if( nLen < 512 )
		_LOG("wheeledshifter:recv(%s)\n",BYTE2HEX( pData, nLen ).c_str());
	return eOK;
}


ErrorCode WheeledShifter::Update(uint32 dtTime)
{
	m_serial.Update( dtTime );
	return eOK;
}


ErrorCode WheeledShifter::Move( MoveType moveType )
{
	uint8 data = (uint8 )moveType;
	m_serial.SendBData(  &data, 1);
	_LOG("Move %d\n",uint8(data) );
	return eOK;
}